#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"

using namespace std::chrono_literals;

class TurtirCircleNode :public rclcpp::Node
{
    private:

        // 发布者智能指针
        rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
        // 订阅者智能指针
        rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscriber_; 

        double trage_x_{1.0};
        double trage_y_{1.0};
        double k_{1.0};//比例系数
        double speed_max_{3.0};//最大速度

    public:
        // explicit防止隐式转换
        explicit TurtirCircleNode(const std::string& node_name ):Node(node_name)
        {
            // 创建发布者
            publisher_=this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
            
            subscriber_=this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&TurtirCircleNode::sub_callback,
            this,std::placeholders::_1));
            
        }
    
        void sub_callback(const turtlesim::msg::Pose::SharedPtr pose)
        {
            // 获取当前位置
            auto x=pose->x;
            auto y=pose->y;
            RCLCPP_INFO(get_logger(),"当前x=%f，y=%f",x,y);

            // 计算海龟和目标的差距

            auto ditance=std::sqrt(
                (trage_x_-x)*(trage_x_-x)+
                (trage_y_-y)*(trage_y_-y)
            );
            auto angle=std::atan2((trage_y_-y),(trage_x_-x))-pose->theta;

            // 控制策略
            auto msg =geometry_msgs::msg::Twist();
            if(ditance>0.1){
                if(fabs(angle)>0.2){
                    msg.angular.z=fabs(angle);
                }else{
                    msg.linear.x=k_*ditance;
                }
            }
            
            if (msg.linear.x> speed_max_){
                msg.linear.x= speed_max_;
            }
            publisher_->publish(msg);
        }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc,argv);
    auto node =std::make_shared<TurtirCircleNode> ("TurtirControl");
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}